uint16_t len = 0;

void SendBytes(uint8_t Buffer[], uint8_t length)
{
  for(int i=0;i<length;i++)
  {
    SerWrite(Buffer[i]); // send message to telemetry port.
  }
}

// 1Hz is good.
void sendMAVLinkHeartBeat()
{
  mavlink_msg_heartbeat_pack(system_id,component_id,&msg,system_type,autopilot_type);
  len = mavlink_msg_to_send_buffer(buf,&msg);
  SendBytes(buf,len);
}

// System state.
//enum MAV_MODE
//{
//    MAV_MODE_UNINIT = 0,     ///< System is in undefined state
//    MAV_MODE_LOCKED = 1,     ///< Motors are blocked, system is safe
//    MAV_MODE_MANUAL = 2,     ///< System is allowed to be active, under manual (RC) control
//    MAV_MODE_GUIDED = 3,     ///< System is allowed to be active, under autonomous control, manual setpoint
//    MAV_MODE_AUTO =   4,     ///< System is allowed to be active, under autonomous control and navigation
//    MAV_MODE_TEST1 =  5,     ///< Generic test mode, for custom use
//    MAV_MODE_TEST2 =  6,     ///< Generic test mode, for custom use
//    MAV_MODE_TEST3 =  7,     ///< Generic test mode, for custom use
//    MAV_MODE_READY =  8,     ///< System is ready, motors are unblocked, but controllers are inactive
//    MAV_MODE_RC_TRAINING = 9 ///< System is blocked, only RC valued are read and reported back
//};
//enum MAV_NAV
//{
//    MAV_NAV_GROUNDED = 0,
//    MAV_NAV_LIFTOFF,
//    MAV_NAV_HOLD,
//    MAV_NAV_WAYPOINT,
//    MAV_NAV_VECTOR,
//    MAV_NAV_RETURNING,
//    MAV_NAV_LANDING,
//    MAV_NAV_LOST,
//    MAV_NAV_LOITER
//};
//enum MAV_MODE
//{
//    MAV_MODE_UNINIT = 0,     ///< System is in undefined state
//    MAV_MODE_LOCKED = 1,     ///< Motors are blocked, system is safe
//    MAV_MODE_MANUAL = 2,     ///< System is allowed to be active, under manual (RC) control
//    MAV_MODE_GUIDED = 3,     ///< System is allowed to be active, under autonomous control, manual setpoint
//    MAV_MODE_AUTO =   4,     ///< System is allowed to be active, under autonomous control and navigation
//    MAV_MODE_TEST1 =  5,     ///< Generic test mode, for custom use
//    MAV_MODE_TEST2 =  6,     ///< Generic test mode, for custom use
//    MAV_MODE_TEST3 =  7,     ///< Generic test mode, for custom use
//    MAV_MODE_READY =  8,     ///< System is ready, motors are unblocked, but controllers are inactive
//    MAV_MODE_RC_TRAINING = 9 ///< System is blocked, only RC valued are read and reported back
//};
//
//enum MAV_STATE
//{
//    MAV_STATE_UNINIT = 0,
//    MAV_STATE_BOOT,
//    MAV_STATE_CALIBRATING,
//    MAV_STATE_STANDBY,
//    MAV_STATE_ACTIVE,
//    MAV_STATE_CRITICAL,
//    MAV_STATE_EMERGENCY,
//    MAV_STATE_POWEROFF
//};
void sendMAVLinkStatus()
{
  mavlink_sys_status_t status;
  status.load = 500;                  ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
  status.mode = sys_mode;           ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h
  status.nav_mode = nav_mode;      ///< Navigation mode, see MAV_NAV_MODE ENUM
  status.packet_drop = 0;            ///< Dropped packets (packets that were corrupted on reception on the MAV)
  status.vbat = 10500;              ///< Battery voltage, in millivolts (1 = 1 millivolt)
  status.status = sys_state;      ///< System status flag, see MAV_STATUS ENUM
  status.battery_remaining = 912;   ///< Remaining battery energy: (0%: 0, 100%: 1000)
  mavlink_msg_sys_status_encode(system_id, MAV_COMP_ID_IMU, &msg, &status);
  len = mavlink_msg_to_send_buffer(buf,&msg);
  SendBytes(buf,len);
}

// IMU
void sendMAVLinkIMU()
{
  mavlink_attitude_t attitude;
  attitude.usec = 0;
  attitude.roll = roll;
  attitude.pitch = pitch;
  attitude.yaw = yaw;
  attitude.rollspeed = Omega[0];
  attitude.pitchspeed = Omega[1];
  attitude.yawspeed = Omega[2];
  
  mavlink_msg_attitude_encode(system_id, MAV_COMP_ID_IMU, &msg, &attitude);
  len = mavlink_msg_to_send_buffer(buf,&msg);
  SendBytes(buf,len);
}

// GPS
void sendMAVLinkGPS()
{
  mavlink_global_position_int_t pos;
  pos.alt = press_sonar_altitude;      // SONAR
  pos.lat = gps.latitude;//*1E7;
  pos.lon = gps.longitude;//*1E7;
  //pos.vx = sin(yaw)*10.0f*100.0f;
  //pos.vy = 0;
  //pos.vz = altPer100ms*10.0f*100.0f*zsign*-1.0f;
  mavlink_msg_global_position_int_encode(system_id, MAV_COMP_ID_IMU, &msg, &pos);
  len = mavlink_msg_to_send_buffer(buf,&msg);
  SendBytes(buf,len);
}

// RC
// Control value.
